#ifndef ROBORTS_DECISION_CHASE_BEHAVIOR_H
#define ROBORTS_DECISION_CHASE_BEHAVIOR_H

#include "io/io.h"

#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../proto/decision.pb.h"

#include "line_iterator.h"

namespace roborts_decision {
class ChaseBehavior {
 public:
  ChaseBehavior(ChassisExecutor* &chassis_executor,
                Blackboard* &blackboard,
                const std::string & proto_file_path) : chassis_executor_(chassis_executor),
                                                       blackboard_(blackboard) {}

  void Run() {

    auto executor_state = Update();

    auto robot_map_pose = blackboard_->GetRobotMapPose();
    if (in_circle==0 && executor_state != BehaviorState::RUNNING && executor_state != BehaviorState::FAILURE) {

      chase_buffer_ = blackboard_->GetAimPosition();
    //  auto dx = chase_buffer_.pose.position.x - robot_map_pose.pose.position.x;
  //    auto dy = chase_buffer_.pose.position.y - robot_map_pose.pose.position.y;

/*      if (std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)) <= 1.4) {
        chassis_executor_->Cancel();
        return;
      }*/ 
     // else{
        geometry_msgs::PoseStamped reduce_goal;
        reduce_goal.pose.orientation = robot_map_pose.pose.orientation;
        reduce_goal.header.frame_id = "map";
        reduce_goal.header.stamp = ros::Time::now();
	reduce_goal.pose.position.x=0.5*(chase_buffer_.pose.position.x+robot_map_pose.pose.position.x);
        reduce_goal.pose.position.y=0.5*(chase_buffer_.pose.position.y+robot_map_pose.pose.position.y);
        reduce_goal.pose.position.z = 1;
        chassis_executor_->Execute(reduce_goal, Behaviortype::CHASE);
     // }
    }
    else if(in_circle||executor_state == BehaviorState::FAILURE)
{
if(in_circle==0)
    {
       begin_time = ros::Time::now();
     }
     in_circle=1;
     roborts_msgs::TwistAccel t;
      t.twist.angular.z = 1.57;
      chassis_executor_->Execute(t);
       if(ros::Time::now().sec-begin_time.sec>=4)in_circle=0;
}
  }

  void Cancel() {
    chassis_executor_->Cancel();
  }

  BehaviorState Update() {
    return chassis_executor_->Update();
  }

/*  void SetGoal(geometry_msgs::PoseStamped chase_goal) {
    chase_goal_ = chase_goal;
  }*/

  ~ChaseBehavior() = default;

 private:
  //! executor
  ChassisExecutor* const chassis_executor_;

  //! perception information
  Blackboard* const blackboard_;

  //! chase goal
  //geometry_msgs::PoseStamped chase_goal_;

  //! chase buffer
  geometry_msgs::PoseStamped chase_buffer_;
  //geometry_msgs::PoseStamped aim_pose;
  //unsigned int chase_count_;

  //! cancel flag
  //bool cancel_goal_;
    int in_circle;
  ros::Time begin_time;
};
}

#endif //ROBORTS_DECISION_CHASE_BEHAVIOR_H
